import RPi.GPIO as GPIO
import time
import sys
import os

import_path = os.path.dirname( os.path.dirname( os.path.abspath(__file__) ) )
sys.path.append( import_path)

#from infraredistance.us100carUltrasound import infrared


class motorContler:
    def __init__(self):

        GPIO.setmode(GPIO.BCM)

        #定义引脚
        self.PWMA = 17
        self.AIN1 = 22
        self.AIN2 = 27
        self.PWMB = 13
        self.BIN1 = 26
        self.BIN2 = 19

        self.PWMAT = 21
        self.AIN2T = 20
        self.AIN1T = 16
        self.BIN1T = 12
        self.BIN2T = 24
        self.PWMBT = 23


        #设置 GPIO 的工作方式
        GPIO.setup(self.PWMA, GPIO.OUT)
        GPIO.setup(self.AIN1, GPIO.OUT)
        GPIO.setup(self.AIN2, GPIO.OUT)
        GPIO.setup(self.PWMB, GPIO.OUT)
        GPIO.setup(self.BIN1, GPIO.OUT)
        GPIO.setup(self.BIN2, GPIO.OUT)
        self.pwma = GPIO.PWM(self.PWMA, 300)
        self.pwmb = GPIO.PWM(self.PWMB, 300)



        #设置 GPIO 的工作方式
        GPIO.setup(self.PWMAT, GPIO.OUT)
        GPIO.setup(self.AIN1T, GPIO.OUT)
        GPIO.setup(self.AIN2T, GPIO.OUT)
        GPIO.setup(self.PWMBT, GPIO.OUT)
        GPIO.setup(self.BIN1T, GPIO.OUT)
        GPIO.setup(self.BIN2T, GPIO.OUT)
        self.pwmat = GPIO.PWM(self.PWMAT, 300)
        self.pwmbt = GPIO.PWM(self.PWMBT, 300)



    def goForward(self, speed):
        # 前进
        if(speed>=0):
            GPIO.output(self.AIN1, GPIO.LOW)
            GPIO.output(self.AIN2, GPIO.HIGH)
            GPIO.output(self.BIN1, GPIO.LOW)
            GPIO.output(self.BIN2, GPIO.HIGH)
            self.pwma.start(speed)
            self.pwmb.start(speed)

            GPIO.output(self.AIN1T, GPIO.LOW)
            GPIO.output(self.AIN2T, GPIO.HIGH)
            GPIO.output(self.BIN1T, GPIO.LOW)
            GPIO.output(self.BIN2T, GPIO.HIGH)
            self.pwmat.start(speed)
            self.pwmbt.start(speed)
            time.sleep(0.02)
        else:
            GPIO.output(self.AIN2, GPIO.LOW)
            GPIO.output(self.AIN1, GPIO.HIGH)
            GPIO.output(self.BIN2, GPIO.LOW)
            GPIO.output(self.BIN1, GPIO.HIGH)
            self.pwma.start(-speed)
            self.pwmb.start(-speed)

            GPIO.output(self.AIN2T, GPIO.LOW)
            GPIO.output(self.AIN1T, GPIO.HIGH)
            GPIO.output(self.BIN2T, GPIO.LOW)
            GPIO.output(self.BIN1T, GPIO.HIGH)
            self.pwmat.start(-speed)
            self.pwmbt.start(-speed)
            time.sleep(0.02)

    # 左转或右转（大于零左转，小于零右转）
    def turnLeft(self, speed):
        if(speed>=0):
            GPIO.output(self.AIN2, GPIO.HIGH)
            GPIO.output(self.AIN1, GPIO.LOW)
            GPIO.output(self.BIN1, GPIO.HIGH)
            GPIO.output(self.BIN2, GPIO.LOW)
            self.pwma.start(speed)
            self.pwmb.start(speed)

            GPIO.output(self.AIN2T, GPIO.LOW)
            GPIO.output(self.AIN1T, GPIO.HIGH)
            GPIO.output(self.BIN1T, GPIO.LOW)
            GPIO.output(self.BIN2T, GPIO.HIGH)
            self.pwmat.start(speed)
            self.pwmbt.start(speed)
            time.sleep(0.02)
        else:
            GPIO.output(self.AIN1, GPIO.HIGH)
            GPIO.output(self.AIN2, GPIO.LOW)
            GPIO.output(self.BIN2, GPIO.HIGH)
            GPIO.output(self.BIN1, GPIO.LOW)
            self.pwma.start(-speed)
            self.pwmb.start(-speed)

            GPIO.output(self.AIN1T, GPIO.LOW)
            GPIO.output(self.AIN2T, GPIO.HIGH)
            GPIO.output(self.BIN2T, GPIO.LOW)
            GPIO.output(self.BIN1T, GPIO.HIGH)
            self.pwmat.start(-speed)
            self.pwmbt.start(-speed)
            time.sleep(0.02)

    # 掉头（大于零从左掉头，小于零从右掉头）
    def turnAround(self, speed):
        if (speed >= 0):
            GPIO.output(self.AIN1T, GPIO.LOW)
            GPIO.output(self.AIN2T, GPIO.HIGH)
            GPIO.output(self.BIN2T, GPIO.LOW)
            GPIO.output(self.BIN1T, GPIO.HIGH)

            GPIO.output(self.AIN2, GPIO.HIGH)
            GPIO.output(self.AIN1, GPIO.LOW)
            GPIO.output(self.BIN1, GPIO.HIGH)
            GPIO.output(self.BIN2, GPIO.LOW)

            self.pwmat.start(speed)
            self.pwmbt.start(speed)
            self.pwma.start(speed)
            self.pwmb.start(speed)
            time.sleep(0.02)
        else:
            GPIO.output(self.AIN2T, GPIO.LOW)
            GPIO.output(self.AIN1T, GPIO.HIGH)
            GPIO.output(self.BIN1T, GPIO.LOW)
            GPIO.output(self.BIN2T, GPIO.HIGH)
            GPIO.output(self.AIN1, GPIO.HIGH)
            GPIO.output(self.AIN2, GPIO.LOW)
            GPIO.output(self.BIN2, GPIO.HIGH)
            GPIO.output(self.BIN1, GPIO.LOW)
            self.pwma.start(-speed)
            self.pwmb.start(-speed)
            self.pwmat.start(-speed)
            self.pwmbt.start(-speed)
            time.sleep(0.02)


    def motorStop(self):
        GPIO.output(self.AIN1, GPIO.LOW)
        GPIO.output(self.AIN2, GPIO.LOW)
        GPIO.output(self.BIN1, GPIO.LOW)
        GPIO.output(self.BIN2, GPIO.LOW)

        GPIO.output(self.AIN1T, GPIO.LOW)
        GPIO.output(self.AIN2T, GPIO.LOW)
        GPIO.output(self.BIN1T, GPIO.LOW)
        GPIO.output(self.BIN2T, GPIO.LOW)

        self.pwma.stop()
        self.pwmb.stop()

        self.pwmat.stop()
        self.pwmbt.stop()
if __name__ == '__main__':
    try:
        while True:
            # 以20%的速度前进
            #motorContler().goForward(-20)
            motorContler().goForward(20)
            #motorContler().turnLeft(50)
            #motorContler().turnLeft(-50)
            #motorContler().turnAround(35)
            #motorContler().turnAround(-35)


        # Reset by pressing CTRL + C
    except KeyboardInterrupt:
        print("Measurement stopped by User")
        motorContler().motorStop()
        GPIO.cleanup()



